#include <Arduino.h>
#include "Esp32PcntEncoder.h"
#include "Esp32McpwmMotor.h"
#include "PidController.h"
#include "Kinematics.h"

#include <WiFi.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <geometry_msgs/msg/twist.h> //获取小车的线速度和角度的接口

#include <nav_msgs/msg/odometry.h> //获取里程计消息接口
#include <micro_ros_utilities/string_utilities.h> //用于简化在资源受限的嵌入式系统中处理 ROS 2 字符串消息


Esp32PcntEncoder encoders[2];//存储脉冲数
Esp32McpwmMotor motor; //初始化电机
PidController pid_controller[2]; //pid控制器来控制俩个电机
Kinematics kinematics;//运动学的正逆解来获得整车的线速度，角速度，以及左右轮的线速度和角速度

// int64_t last_ticks[2]; //记录上一次读取的计数器数值，即脉冲数
// int32_t delta_ticks[2]; //记录俩次读取的脉冲数的差值
// int64_t last_update_time; //记录上次更新的时间
// float current_speeds[2]; //记录当前的速度


float targer_linger_speed = 0.0; //目标线速度的大小为50,单位为mm/s
float target_angular_speed = 0.0; //目标角速度大小为0.1f，单位为rad/s
float out_left_speed; //左轮速度
float out_right_speed; //右轮速度

rcl_allocator_t allocator; //管理ROS 2运行时的动态内存分配
rclc_support_t support; //保存ROS 2的运行时上下文和初始化支持信息
rclc_executor_t executor; //管理ROS 2事件的执行器,负责调度回调函数
rcl_node_t node; //一个ROS 2节点

rcl_subscription_t subscriber; //消息订阅者
geometry_msgs__msg__Twist sub_msg; //存储订阅到的速度消息

rcl_publisher_t odom_publisher; //里程计消息发布者
nav_msgs__msg__Odometry odom_msg; //里程计消息
rcl_timer_t timer; //定时器，可以定时调用某个函数

//里程计回调函数来获取里程计信息
void callback_publisher(rcl_timer_t* timer,int64_t last_call_time)
{
  odom_t odom = kinematics.get_odom(); //获取里程计
  int64_t stamp = rmw_uros_epoch_millis(); //获取当前时间

  //获取时间消息
  odom_msg.header.stamp.sec = static_cast<uint32_t>(stamp / 1000); //获得秒部分
  odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp / 1000) * 1e6); //获得纳秒部分

  //获取姿态消息
  odom_msg.pose.pose.position.x = odom.x; //x方向的姿态
  odom_msg.pose.pose.position.y = odom.y; //y方向的姿态

  // Z 轴的旋转角度（欧拉角）转换为 四元数
  odom_msg.pose.pose.orientation.w = cos(odom.angle * 0.5);
  odom_msg.pose.pose.orientation.x = 0;
  odom_msg.pose.pose.orientation.y = 0;
  odom_msg.pose.pose.orientation.z = sin(odom.angle * 0.5);

  //获取角速度和线速度
  odom_msg.twist.twist.angular.z = odom.angular_speed;
  odom_msg.twist.twist.linear.x = odom.linear_speed;

  //发布里程计
  //用于将odom_msg消息通过odom_publisher发布到指定的/odom话题。第三个参数NULL表示使用默认的内存分配器
  if(rcl_publish(&odom_publisher,&odom_msg,NULL) != RCL_RET_OK)
  {
    Serial.printf("error:odom publisher failed!\n");
  }


}

//速度回调函数来获取速度信息
void twist_callback(const void* msg_in)
{
  //将得到的消息指针转化为geometry_msgs__msg__Twist
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;

  //运动学逆解并设置速度
  //线速度*1000是将ms/s转化为m/s
  kinematics.kinematics_inverse(twist_msg->linear.x * 1000, twist_msg->angular.z, &out_left_speed, &out_right_speed);
  //更新速度
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
}


void micro_ros_task(void *parameter)
{
  //创建储存agentip的对象
  IPAddress agent_ip;
  //解析ip地址
  agent_ip.fromString("192.168.207.21");

  //配置Micro-ROS的WiFi传输参数
  //wifi_name wifi的名称
  //WIFI_PASSWORD wifi的密码
  //agent_ip 目标的ip地址
  //默认端口号
  set_microros_wifi_transports("wzj","1234566778",agent_ip,8888);
  //等待2秒，确保wifi和agent连接
  delay(2000);
  // 获取默认内存分配器
  allocator = rcl_get_default_allocator();

  // 初始化ROS 2支持结构体
  //support存储初始化后的上下文和配置
  //allocator内存分配器
  rclc_support_init(&support,0,NULL,&allocator);

  //创建名为"fishbot_motion_control"的ROS 2节点，并且关联到支持结构体
  rclc_node_init_default(&node,"fishbot_motion_control","",&support);

  //用来设置预分配的回调语柄
  unsigned int num_handles = 2;

  // 初始化执行器
  //executor执行器对象指针
  //support.context ROS 2上下文
  //num_handles预分配的回调句柄数量
  //allocator 内存分配器
  rclc_executor_init(&executor,&support.context,num_handles,&allocator);

  //初始化订阅者并将他添加到执行器中
  rclc_subscription_init_best_effort(
    &subscriber, //订阅者对象的指针
    &node, //指向已创建节点的指针
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist), //获取 geometry_msgs/msg/Twist 消息的类型支持
    "/myrobot_control" //订阅者监听的话题名称
  );

  rclc_executor_add_subscription(&executor, //指向已初始化的执行器实例
                                &subscriber, //指向消息订阅者
                                &sub_msg, // 订阅的速度消息
                                &twist_callback, //回调函数
                                ON_NEW_DATA //仅在接收到新消息时触发回调
  );

  //获取header.frame_id和child_frame_id
  //header.frame_id就是全局坐标系
  //child_frame_id就是机器人坐标系
  odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id,"odom");
  odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id,"base_footprint");

  //初始化一个以“最佳效果”（Best Effort）服务质量（QoS）策略发布的里程计话题/odom
  rclc_publisher_init_best_effort(
    &odom_publisher,&node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs,msg,Odometry),"/odom" // 确保消息类型与ROS 2中间件兼容，避免类型不匹配导致的发布失败
    );

  //如果没有进行时间同步，进入函数进行时间同步
  while(!rmw_uros_epoch_synchronized())
  {
    rmw_uros_sync_session(1000); //尝试进行时间同步
    delay(10);
  }

  //创建定时器，每50ms发布调用callback_publisher里程计话题
  rclc_timer_init_default(&timer,&support,RCL_MS_TO_NS(50),callback_publisher);

  //
  rclc_executor_add_timer(&executor,&timer);


  // 阻塞式启动执行器，持续处理ROS事件
  rclc_executor_spin(&executor);
}


void setup()
{
  // 初始化串口
  Serial.begin(115200);
  // 初始化编码器
  encoders[0].init(0, 32, 33);  // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25);  // 初始化第二个编码器，使用GPIO 26和25连接
  // 初始化电机驱动器
  motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23
  motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13
  // 设置电动机的速度
  //motor.updateMotorSpeed(0, 50);
  //motor.updateMotorSpeed(1, 50);
  //设置pid的值
  pid_controller[0].update_pid(0.625,0.125,0.0);
  pid_controller[1].update_pid(0.625,0.125,0.0);
  //限制速度的上下限
  pid_controller[0].out_limit(-100,100);
  pid_controller[1].out_limit(-100,100);
  //初始化轮子间距和电动机参数
  kinematics.set_wheel_distance(175); //设置轮子间距为175
  kinematics.set_motor_param(0,0.1051566); //左轮每一轮走了0.1051566
  kinematics.set_motor_param(1,0.1051566); //右轮每一轮走了0.1051566
  //运动学逆解，，将线速度和角速度转换成左右轮的速度
  kinematics.kinematics_inverse(targer_linger_speed,target_angular_speed,&out_left_speed,&out_right_speed);
  //更新数据
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);


  //设置目标速度
  // pid_controller[0].update_target(100);
  // pid_controller[1].update_target(100);

  xTaskCreate(micro_ros_task, //任务函数
              "micro_ros", //任务名称
              10240,      //任务堆栈大小
              NULL,       //传递给任务函数的参数
              1,          //任务优先级
              NULL        //任务句柄
              );



}

void loop()
{
  delay(10); //等待10ms
  //输入左右轮脉冲数，当前时间，输出更新电机速度和编码器数据
  kinematics.update_motor_speed(millis(),encoders[0].getTicks(),encoders[1].getTicks());
  //传递当前电机的速度
  motor.updateMotorSpeed(0,pid_controller[0].update(kinematics.get_motor_speed(0)));
  motor.updateMotorSpeed(1,pid_controller[1].update(kinematics.get_motor_speed(1)));

  Serial.printf("x=%f,y=%f,angle=%f\n",kinematics.get_odom().x,kinematics.get_odom().y,kinematics.get_odom().angle);

}